#ifndef __slam_slamcontroller_h__
#define __slam_slamcontroller_h__

#include <ros/ros.h>
#include <driver/Movement.h>
#include <simulation/robot.h>
#include <sensor/IR.h>
#include <sensor/tag.h>
#include "map/mapper.h"
#include "target_tag.h"
#include <vector>
#include "SLAM/Probabilities.h"
#include "path/path.h"
#include "SLAM/Directions.h"
#include <SLAM/SwitchPhase.h>
#include <SLAM/RecordedTags.h>
#include <SLAM/Waypoints.h>

typedef struct {
	float x;
	float y;
	float z;
	float angle;
} IRSensorInfo;

typedef struct {
	float mean;
	float std_dev;
	float height;
} SensorRayParameters;

class SLAMController
{
public:
	SLAMController();
	~SLAMController();

private:
	void sensor_callback(const sensor::IR::ConstPtr& msg);
	void sensor_tag_callback(const sensor::tag::ConstPtr& msg);
	bool probabilities_request(SLAM::Probabilities::Request& req, SLAM::Probabilities::Response& res);
	bool path_request(SLAM::Directions::Request& req, SLAM::Directions::Response& res);
	void switch_phase_callback(const SLAM::SwitchPhase::ConstPtr& msg);
	bool recorded_tags_request(SLAM::RecordedTags::Request& req, SLAM::RecordedTags::Response& res);
	bool waypoints_request(SLAM::Waypoints::Request& req, SLAM::Waypoints::Response& res);

	void deleteAllTags();

	void loadMapDataFromFile();
	void saveMapDataToFile();

	void renderRay(float from_x, float from_y, float to_x, float to_y);
	void renderTags();

	void getSensorInformation();

	bool robot_active_;

	Mapper mapper_;
	Robot robot_;
	Path path_;

	ros::Publisher distance_ray_publisher_;
	ros::Subscriber sensor_ir_subscriber_;
	ros::Subscriber sensor_tag_subscriber_;
	ros::ServiceClient tracker_client_;
	ros::ServiceClient sensor_info_client_;
	ros::ServiceServer probabilities_server;
	ros::ServiceServer path_server;
	ros::ServiceServer recorded_tags_server_;
	ros::ServiceServer waypoints_server_;
	ros::Subscriber switch_phase_subscriber_;

	std::vector<IRSensorInfo> sensor_infos_;
	std::vector<TargetTag *> tags_;
	SensorRayParameters *last_sensor_readings_;

	TargetTag *zeroTagHack_;
};

#endif
